#include "stm32f10x.h"                  // Device header
#include "mpu6050_i2c.h"
#include "main.h"


#define I2C1_SCL_PORT    GPIOB
#define I2C1_SCL_PIN     GPIO_Pin_6
#define I2C1_SDA_PORT    GPIOB
#define I2C1_SDA_PIN     GPIO_Pin_7

#define I2C1_SCL_HIGH()  GPIO_SetBits(I2C1_SCL_PORT, I2C1_SCL_PIN)
#define I2C1_SCL_LOW()   GPIO_ResetBits(I2C1_SCL_PORT, I2C1_SCL_PIN)
#define I2C1_SDA_HIGH()  GPIO_SetBits(I2C1_SDA_PORT, I2C1_SDA_PIN)
#define I2C1_SDA_LOW()   GPIO_ResetBits(I2C1_SDA_PORT, I2C1_SDA_PIN)
#define I2C1_SDA_READ()  GPIO_ReadInputDataBit(I2C1_SDA_PORT, I2C1_SDA_PIN)
#define I2C1_DELAY()     i2c_delay()


static void i2c_delay(void)
{
    delay_us(5);
}

static void i2c_start(void)
{
    I2C1_SDA_HIGH();
    I2C1_SCL_HIGH();
    I2C1_DELAY();
    I2C1_SDA_LOW();
    I2C1_DELAY();
    I2C1_SCL_LOW();
}

static void i2c_stop(void)
{
    I2C1_SDA_LOW();
    I2C1_DELAY();
    I2C1_SCL_HIGH();
    I2C1_DELAY();
    I2C1_SDA_HIGH();
    I2C1_DELAY();
}

static bool i2c_write_byte(uint8_t data)
{
    for(uint8_t i = 0; i < 8; i++)
    {
        if(data & 0x80)
            I2C1_SDA_HIGH();
        else
            I2C1_SDA_LOW();
        I2C1_DELAY();
        I2C1_SCL_HIGH();
        I2C1_DELAY();
        I2C1_SCL_LOW();
        data <<= 1;
    }
    I2C1_SDA_HIGH();
    I2C1_DELAY();
    I2C1_SCL_HIGH();
    I2C1_DELAY();
    if(I2C1_SDA_READ())
    {
        I2C1_SCL_LOW();
        return false;
    }
    I2C1_SCL_LOW();
    I2C1_DELAY();
    return true;
}

static uint8_t i2c_read_byte(bool ack)
{
    uint8_t data = 0;
    I2C1_SDA_HIGH();
    for( uint8_t i = 0; i < 8; i++)
    {
        data <<= 1;
        I2C1_SCL_HIGH();
        I2C1_DELAY();
        if (I2C1_SDA_READ())
            data |= 0x01;
        I2C1_SCL_LOW();
        I2C1_DELAY();
    }
    if( ack)
        I2C1_SDA_LOW();
    else
        I2C1_SDA_HIGH();
    I2C1_DELAY();
    I2C1_SCL_HIGH();
    I2C1_DELAY();
    I2C1_SCL_LOW();
    I2C1_SDA_HIGH();
    I2C1_DELAY();
    return data;
}

static void mpu6050_i2c_io_init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
    GPIO_InitStructure.GPIO_Pin = I2C1_SCL_PIN;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(I2C1_SCL_PORT,&GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
    GPIO_InitStructure.GPIO_Pin = I2C1_SDA_PIN;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(I2C1_SDA_PORT,&GPIO_InitStructure);
}

void mpu6050_i2c_init(void)
{
    mpu6050_i2c_io_init();
}

bool mpu6050_i2c_write(uint8_t addr, uint8_t reg, uint8_t *data, uint16_t length)
{
    i2c_start();
    if(!i2c_write_byte(addr << 1))
    {
        i2c_stop();
        return false;
    }
    i2c_write_byte(reg);
    for(uint16_t i = 0; i < length; i++)
    {
        i2c_write_byte(data[i]);
    }
    i2c_stop();
    return true;
}
bool mpu6050_i2c_read(uint8_t addr, uint8_t reg, uint8_t *data, uint16_t length)
{
    i2c_start();
    if(!i2c_write_byte(addr << 1))
    {
        i2c_stop();
        return false;
    }
    i2c_write_byte(reg);
    i2c_start();
    i2c_write_byte((addr << 1) | 0x01);
    for(uint16_t i = 0; i < length; i++)
    {
        data[i] = i2c_read_byte(i == length - 1 ? false : true);
    }
    i2c_stop();
    return true;
}
